#pragma once
// #autor:yxj
#include "rclcpp/rclcpp.hpp"
#include <agv_peripheral_controller/toolKit.h>
#include <agv_msgs/msg/read_in_puts.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_srvs/srv/outputs_new.hpp>

// 急停
#define EMG_BUTTON "emergencyButton"
#define EMG_BUTTON_2 "emergencyButton2"
// 液压举升
#define UP_LIMIT "upLimit"
#define DOWN_LIMIT "downLimit"
#define CONTROL_IO_UP "upControl"
#define CONTROL_IO_DOWN "downControl"

enum Switch { ioLiftUp, ioLiftDown, normalLiftUp, normalLiftDown };
enum class SwitchType { ioLiftType, normalLiftType };


#define SET_ONE_BIT(x) (((uint64_t)1) << (x))
#define TEST_BIT(x, i) ((x & SET_ONE_BIT(i)) != 0)
#define SET_N_BIT(x, n) (x | ((uint64_t)(1) << n))

// 手自动模式获取比特位(agv_info)
#define MANUL_BIT 4
#define HALF_BIT 22
#define AUTO_ONLINE_BIT 23

namespace agv_peripheral
{
    // 定义输入IO点类型
    using INPUT_IO_TYPE = CDataTimeStamp<agv_msgs::msg::PairTypeInt>;
    using OUTPUT_IO_TYPE = agv_msgs::msg::PairTypeInt;
    using IO_GROUP = std::vector<OUTPUT_IO_TYPE>;

    // 输入检测函数
    inline bool inputIo(INPUT_IO_TYPE &inputIo)
    {
        bool ret = false;
        bool timeOut = inputIo.TimeOut(300);
        bool pressed = inputIo.Get().value;

        if(timeOut){
            DEBUG_ERROR_OUT("inputIo:data time out:key=" << inputIo.Get().key);
            return ret;
        }
        return pressed;
    }

    // CIoLift类
    class CIoLift
    {
    public:
        CIoLift() = default;
        ~CIoLift() = default;

        // 使用ROS2客户端类型
        bool init(rclcpp::Client<agv_srvs::srv::OutputsNew>::SharedPtr& client);
        bool up();
        bool down();
        bool stop();
        bool loadStatus();

        INPUT_IO_TYPE upLimit;
        INPUT_IO_TYPE downLimit;
        IO_GROUP ctrl;

    private:
        bool upTarget();
        bool downTarget();
        
        bool outputIo(IO_GROUP outIoGroup);
    
        IO_GROUP m_outIoRecord;
        rclcpp::Client<agv_srvs::srv::OutputsNew>::SharedPtr m_client;
    };

    // CIoLifterController
    class CIoLifterController 
    {
    public:
        explicit CIoLifterController(std::shared_ptr<rclcpp::Node> n);
        ~CIoLifterController();
        
        bool initFinish();
        bool init();
        bool up();
        bool down();
        bool stop();
        bool loadStatus();
        bool emgPressed();
    private:
        void readInputNewCallBack(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr& msg);
        void updateInutIoNew(const agv_msgs::msg::ReadInPutsNew &msg, INPUT_IO_TYPE &inputIoObj);
        
        std::shared_ptr<rclcpp::Node> node;    
        CIoLift m_normalLift;
        bool m_init;

        rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr m_subIoNew;
        rclcpp::Client<agv_srvs::srv::OutputsNew>::SharedPtr m_outClient;

        INPUT_IO_TYPE m_emgBtn;
        INPUT_IO_TYPE m_emgBtn_2;
    };
}